#include "coordMath.h"

ECEFvector LLHtoECEF(LLHvector coord) {

    double f = (C_WGS84_a - C_WGS84_b) / C_WGS84_a;
    double e = sqrt( 2 * f - f * f );
    double N = C_WGS84_a / sqrt(1 - e * e * sin(coord.lat) * sin(coord.lat));
    
    ECEFvector result;
    result.x = (N + coord.height) * cos(coord.lat) * cos(coord.lon); 
    result.y = (N + coord.height) * cos(coord.lat) * sin(coord.lon);
    result.z = (N * (1 - e * e) + coord.height) * sin(coord.lat);

    return result;

} /* end LLHtoECEF */
